/// Alvaro Cuno, Mar2008
/// Quaternion definition file

#ifndef _QUATERNION_H_
#define _QUATERNION_H_

#include <cmath>
#include <cmath>
#include <cassert>
#include <iostream>

using namespace std;

template <class Number>
class Quaternion {
   
public:
   /// Default constructor
   Quaternion() {
      q[0]=1.0; q[1]=q[2]=q[3]=0.0;
   }
   /// Copy constructor
   Quaternion(const Quaternion &Q) {
      *this = Q;
   }
   /// Constructor
   Quaternion(Number q0, Number q1, Number q2, Number q3) {
      q[0]=q0; q[1]=q1; q[2]=q2; q[3]=q3;
   }
   /// Vector constructor
   Quaternion(Number q1, Number q2, Number q3) {
      q[0]=0.0; q[1]=q1; q[2]=q2; q[3]=q3;
   }
   /// Assignment operator
   const Quaternion& operator=(const Quaternion &Q) {
      q[0]=Q.q[0]; q[1]=Q.q[1]; q[2]=Q.q[2]; q[3]=Q.q[3];
      return *this;
   }
   /// Quaternion sum operator
   Quaternion operator+(const Quaternion &Q) const {
      Quaternion R(*this);
      R.q[0]+=Q.q[0]; R.q[1]+=Q.q[1]; R.q[2]+=Q.q[2]; R.q[3]+=Q.q[3];
      return R;
   }
   /// Quaternion product operator
   Quaternion operator*(const Quaternion &Q) const {
      Quaternion R; const Quaternion &P = *this;
      R.q[0]=P.q[0]*Q.q[0] - P.q[1]*Q.q[1] - P.q[2]*Q.q[2] - P.q[3]*Q.q[3];
      R.q[1]=P.q[1]*Q.q[0] + P.q[0]*Q.q[1] + P.q[2]*Q.q[3] - P.q[3]*Q.q[2];
      R.q[2]=P.q[2]*Q.q[0] + P.q[0]*Q.q[2] + P.q[3]*Q.q[1] - P.q[1]*Q.q[3];
      R.q[3]=P.q[3]*Q.q[0] + P.q[0]*Q.q[3] + P.q[1]*Q.q[2] - P.q[2]*Q.q[1];
      return R;
      
      //~ Number l = sqrt(R.q[0]*R.q[0] + R.q[1]*R.q[1] + R.q[2]*R.q[2] + R.q[3]*R.q[3]);
      //~ return Quaternion(R.q[0]/l, R.q[1]/l, R.q[2]/l, R.q[3]/l);
   }
   /// read-only specific field operator
   const Number operator[](int i) const {
      assert(i<4);
      return q[i];
   }
   /// Quaternion x scalar product
   Quaternion operator*(Number k) {
      Quaternion Q(*this);
      Q.q[0] *= k; Q.q[1] *= k; Q.q[2] *= k; Q.q[3] *= k;
      return Q;
   }
   
   /// scalar x Quaternion product
   template <class T>
   friend Quaternion<T> operator*(T, const Quaternion<T> &);
   /// output operator
   template <class T>
   friend ostream &operator<<(ostream &, const Quaternion<T> &);
   /// input operator
   template <class T>
   friend istream &operator>>(istream &, const Quaternion<T> &);
   
   /// Instances a quaternion given an unit axis and angle
   static Quaternion fromAxisAngle(Number x, Number y, Number z, Number angle) {
      const Number sina = sin(angle*0.5); /// sina = sin(angle/2)
      const Number &cosa = cos(angle*0.5); /// cosa = cos(angle/2)
      return Quaternion(cosa, x*sina, y*sina, z*sina);
   }
   
private:
   Number q[4]; // The quaternion components {w, x,y,z} = {real, vectorial}
};

/// output operator
template <class Number>
ostream &operator<<(ostream &out, const Quaternion<Number> &Q) {
   out<<Q.q[0]<<" "<<Q.q[1]<<" "<<Q.q[2]<<" "<<Q.q[3];
   return out;
}

/// input operator
template <class Number>
istream &operator>>(istream &in, const Quaternion<Number> &Q) {
   in>>Q.q[0]>>Q.q[1]>>Q.q[2]>>Q.q[3];
   return in;
}

/// scalar x Quaternion product
template <class Number>
Quaternion<Number> operator*(Number k, const Quaternion<Number> &Q) {
   return Quaternion<Number> (k*Q[0], k*Q[1], k*Q[2], k*Q[3]);
}

/// dot product of two quaternions
template <class Number>
Number dot(const Quaternion<Number> &Q, const Quaternion<Number> &R) {
   return Q[0]*R[0] + Q[1]*R[1] + Q[2]*R[2] + Q[3]*R[3];
}

/// quaternion conjugate
template <class Number>
Quaternion<Number> conjugate(const Quaternion<Number> &Q) {
   return Quaternion<Number> (Q[0], -Q[1], -Q[2], -Q[3]);
}

/// quaternion length
template <class Number>
Number length(const Quaternion<Number> &Q) {
   return sqrt(dot(Q,Q));
}

/// quaternion squared length
template <class Number>
Number squaredLength(const Quaternion<Number> &Q) {
   return dot(Q,Q);
}

#endif
